#include <rclcpp/rclcpp.hpp>  // ROS2 C++ API
#include <sensor_msgs/msg/laser_scan.hpp>  // 订阅激光雷达数据
#include <opencv2/highgui.hpp>  // OpenCV 相关
#include "ch6/mapping_2d.h"  // SLAM 处理

class LidarSLAMNode : public rclcpp::Node {
public:
    LidarSLAMNode() : Node("lidar_slam") {
        rclcpp::QoS qos(rclcpp::KeepLast(10)); 
        qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); // 显式设置可靠传输的QoS策略
        // rclcpp::QoS qos_profile = rclcpp::QoS(10).best_effort();
        // 订阅 /scan 话题，回调函数用于处理数据
        lidar_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "/scan", qos,
            std::bind(&LidarSLAMNode::scanCallback, this, std::placeholders::_1));
        this->declare_parameter<std::string>("scan_topic", "/scan");
        scan_topic_ = this->get_parameter("scan_topic").as_string();

        // 初始化 SLAM
        if (!mapping_.Init(true)) {  // 这里可以调整是否启用回环检测
            RCLCPP_ERROR(this->get_logger(), "SLAM 初始化失败！");
            rclcpp::shutdown();
        }
    }

private:
    void scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
        // 将 ROS2 的 LaserScan 数据转换为 SLAM 可用格式
        Scan2d::Ptr scan = std::make_shared<Scan2d>();  
        scan->ranges = msg->ranges;  // 假设 Scan2d 结构体包含 ranges
        scan->range_min = msg->range_min;
        scan->range_max = msg->range_max;
        scan->angle_min = msg->angle_min;
        scan->angle_max = msg->angle_max;
        scan->angle_increment = msg->angle_increment;

        // 传递给 SLAM 进行处理
        if (!mapping_.ProcessScan(scan)) {
            RCLCPP_WARN(this->get_logger(), "SLAM 处理失败！");
        }
    }

    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr lidar_sub_;  // 订阅者
    sad::Mapping2D mapping_;  // SLAM 处理类
    std::string scan_topic_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<LidarSLAMNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
